#include "Control.h"
#include "Delay.h"
#include "IMU.h"
#include "Motor.h"
#include "PID.h"
#include "Sensor.h"
#include "UART.h"
#include "ti/driverlib/dl_gpio.h"
#include "ti_msp_dl_config.h"

int MPU_Init_Data = 0;
uint32_t LED_TIMER = 0;
volatile uint16_t PID_TIMER = 0;
volatile uint16_t MPU6050_TIMER = 0;
volatile int32_t freq = 0;
volatile int32_t freq2 = 0;
volatile float APWM = 0;
volatile float BPWM = 0;
int SPWM = 0;
int HPWM = 0;
int MPWM = 0;
volatile float speed = 0;
volatile int32_t Total_ERROR = 0;
int32_t L_Motor_Distance = 0;
int32_t R_Motor_Distance = 0;
volatile float Distance = 0;
volatile char KEY1_Status = 0;
volatile char KEY2_Status = 0;
volatile char KEY3_Status = 0;
volatile char KEY4_Status = 0;
uint8_t status;
uint8_t KEY_CNT1 = 0;
uint8_t KEY_CNT2 = 0;
uint8_t KEY_CNT3 = 0;
uint8_t KEY_CNT4 = 0;
uint8_t KEY = 0;

// ----------------- 外部变量 ----------------------
extern int32_t gCaptureCnt1;
extern int32_t gCaptureCnt2;

// ---------------- 函数定义区域 -------------------------
float Get_Velocity_Form_Encoder(int32_t encoder);
void Conditional_Judgment(void);
void Straight(int Speed);
void PID_Straight(int ASpeed, int BSpeed, int PWM);
void Turn_Left_90(void);
void Turn_Right_90(void);
void Stop(void);
void Start(void);
void HCSR04_Control(void);
void Sensor_Control(void);
void KEY_Control(void);
float Get_Distance_Form_Encoder(int32_t encoder);
void MPU6050_Control();
void track_eng_eng(int pwma, int pwmb);
int Yaw_Handle(int yaw);
void Yaw_Init(void);
// ------------------- END ------------------------------

//------------------ 任务变量 ----------------------
int Led_Flag = 0;
int TASK2_Step = 0;
uint8_t LastArea = 0;
uint8_t Area = 9;
uint8_t StopArea_5 = 0;
uint8_t StopArea_1 = 0;
//--------------------- END ----------------------

// --------------------- 任务相关 --------------------
void TASk1(void) {

  //   if (Sensor_Data != 0x00) {
  //     Clear_Params(&Motor1_PID);
  //     Clear_Params(&Motor2_PID);
  //     APWM = 0;
  //     BPWM = 0;
  //     Stop();
  //     OLED_ShowSignedNum(4, 11, Distance, 4);
  //     // LED_Flicker();
  //     KEY1_Status = 0;
  //   } else {
  //     // Motor1_PID.target_val = 100;
  //     // Motor2_PID.target_val = 100;
  //   }
  PID_Straight(APWM, BPWM, 0);

  if (L_Motor_Distance >= 1000) {
    Clear_Params(&Motor1_PID);
    Clear_Params(&Motor2_PID);
    APWM = 0;
    BPWM = 0;
    Stop();
  }
}

void TASk2(void) {
  switch (TASK2_Step) {
    Sensor_Data = gw_gray_serial_read();
  case 0:
    Yaw_Init();
    MPU6050_PID.target_val = MPU_Init_Data;
    PID_Straight(30, 30, MPWM);
    if (Sensor_Data != 0x00) {
      Led_Flag = 1;
      TASK2_Step = 1;
    }
    break;
  case 1:
    track_eng_eng(30, 30);
    if (LastArea == 9) {
      Led_Flag = 1;
      TASK2_Step = 2;
    }
    break;
  case 2:
    MPU6050_PID.target_val = MPU_Init_Data - 180;
    PID_Straight(30, 30, MPWM);
    if (Sensor_Data != 0x00) {
      Led_Flag = 1;
      TASK2_Step = 3;
    }
    break;
  case 3:
    track_eng_eng(30, 30);
    if (LastArea == 9) {
      Led_Flag = 1;
      TASK2_Step = 4;
    }
    break;
  }
}

void TASk3(void) {
  static uint8_t TASK3_Step = 0;
  static uint8_t Num = 0;
  int16_t D_Value = 0;

  Motor_PID_Param_Init();

  switch (TASK3_Step) {
  case 0:
    // 判断是否到弯道
    if (Sensor_Data != 0x00) {
      Sensor_PID_Param_Init();
      // ------ 跳转 ------
      TASK3_Step = 1;
      L_Motor_Distance = 0;
      R_Motor_Distance = 0;
    }
    D_Value = Num == 0 ? L_Motor_Distance - R_Motor_Distance
                       : R_Motor_Distance - L_Motor_Distance; // 差值
    if (D_Value < 50) {
      Motor1_PID.target_val = Num == 0 ? 50 : 30;
      Motor2_PID.target_val = Num == 0 ? 30 : 50;
    } else if (D_Value >= 50) {
      Motor1_PID.target_val = Num == 0 ? 30 : 50;
      Motor2_PID.target_val = Num == 0 ? 50 : 30;
    } else if (D_Value == 0) {
      Motor1_PID.target_val = 50;
      Motor2_PID.target_val = 50;
    }
    PID_Straight(APWM, BPWM, 0);
    break;
  case 1:
    D_Value = R_Motor_Distance - L_Motor_Distance; // 差值
                                                   // 判断是否出弯道
    if (Sensor_Data == 0x00) {
      // 清除循迹的PID
      Clear_Params(&Sensor_PID);
      error = 0;
      SPWM = 0;
      if (D_Value == 25) {
        // ------ 跳转 ------
        TASK3_Step = Num == 0 ? 0 : 2;
        Num = 1;
      } else {
        Amend_PID_Param_Init();
        int8_t PWM = PosionPID_realize(&Amend_PID, D_Value - 25);
        PID_Straight(PWM, -PWM, 0);
      }
    } else {
      PID_Straight(APWM, BPWM, SPWM);
    }
    break;
  case 2:
    Stop();
    break;
  case 3:
    break;
  }
}

void TASk4(void) {}

void track_eng_eng(int pwma, int pwmb) {
  Sensor_Data = gw_gray_serial_read();
  if (Sensor_Data == 0x06) {
    Area = 3;
    StopArea_1 = 0;
    StopArea_5 = 0;
    PID_Straight(pwma, pwmb, 0);
  }

  if (Sensor_Data == 0x02) {
    Area = 4;
    StopArea_5 = 0;
    PID_Straight(pwma + pwmb / 3, pwmb, 0);
  }
  if (Sensor_Data == 0x01) {
    // Area = 6;
    StopArea_5 = 0;
    PID_Straight(pwma + pwmb / 3 * 2, pwmb, 0);
  }

  if (Sensor_Data == 0x04) {
    Area = 2;
    StopArea_1 = 0;
    PID_Straight(pwma, pwmb + pwma / 3, 0);
  }
  if (Sensor_Data == 0x08) {
    // Area = 0;
    StopArea_1 = 0;
    PID_Straight(pwma, pwmb + pwma / 3 * 2, 0);
  }

  if (Sensor_Data == 0x00) {
    if (LastArea == 4) {
      Area = 5;
      PID_Straight(pwma + pwmb / 2, pwmb, 0);
    }
    if (LastArea == 2) {
      Area = 1;
      PID_Straight(pwma, pwmb + pwma / 2, 0);
    }
    if (StopArea_5 > 5) {
      StopArea_5 = 0;
      Area = 9;
      PID_Straight(0, 0, 0);
    }
    if (StopArea_1 > 3) {
      StopArea_1 = 0;
      Area = 9;
      PID_Straight(0, 0, 0);
    }
  }

  if (LastArea == 5) {
    StopArea_5++;
  }
  if (LastArea == 1) {
    StopArea_1++;
  }

  LastArea = Area;
}

//----------------------------- 主控制函数 ---------------------------
void Conditional_Judgment(void) {
  Sensor_Control();  // 循迹
  KEY_Control();     // 按键
  MPU6050_Control(); // 陀螺仪
  // HCSR04_Control(); // 超声波

  //--------------------------- 主要逻辑 ------------------------------
  if (KEY1_Status == 1) {
    TASk1();
  } else if (KEY2_Status == 1) {
    TASk2();
  } else if (KEY3_Status == 1) {
    // TASk3();
  } else if (KEY4_Status == 1) {
    // TASk4();

    PID_Straight(50, 50, MPWM);
  }

  // ------------------------- LED ---------------------------------
  if (Led_Flag == 1) {
    DL_GPIO_setPins(OTHER_PORT, OTHER_LED_PIN);
    DL_GPIO_setPins(OTHER_PORT, OTHER_BUZZER_PIN);
  } else {
    DL_GPIO_clearPins(OTHER_PORT, OTHER_LED_PIN);
    DL_GPIO_clearPins(OTHER_PORT, OTHER_BUZZER_PIN);
  }
}
// ---------------------------- 显示部分 ------------------------------
void OLED_Static_Show() {
  OLED_ShowString(1, 1, "freq:");
  OLED_ShowString(2, 1, "er-AP:");
}

void OLED_Loop_Show() {
  OLED_ShowSignedNum(1, 7, freq2, 3);
  OLED_ShowSignedNum(1, 12, freq, 3);
  OLED_ShowNum(4, 15, KEY, 1);
  OLED_ShowSignedNum(2, 7, APWM, 4);
  OLED_ShowSignedNum(2, 12, MPU6050_PID.target_val, 4);
  //   OLED_ShowNum(3, 1, Sensor_PID.Kp * 100, 5);
  //   OLED_ShowNum(3, 7, Sensor_PID.Kd * 100, 5);
  OLED_ShowNum(3, 1, Motor1_PID.Kp * 100, 5);
  OLED_ShowNum(3, 7, Motor1_PID.Ki * 100, 5);
  //   OLED_ShowSignedNum(4, 1, L_Motor_Distance, 4);
  //   OLED_ShowSignedNum(4, 6, R_Motor_Distance, 4);
  // OLED_ShowNum(4, 1, decide, 1);
  //   OLED_ShowSignedNum(4, 1, error, 3);
  //   OLED_ShowSignedNum(4, 6, SPWM, 5);
  //   OLED_ShowBinNum(4, 8, Sensor_Data, 8);
}

//----------------------------- 模块控制 -----------------------------------
void KEY_Control() {
  // 按键1
  if (!DL_GPIO_readPins(KEYS_PORT, KEYS_KEY1_PIN)) {
    // DL_GPIO_clearPins(OTHER_PORT, OTHER_TEST_PIN);
    KEY_CNT1++;
    if (KEY_CNT1 > 3) {
      KEY_CNT1 = 0;
      if (KEY1_Status == 0) {
        KEY1_Status = 1;
        KEY = 1;
        Motor_PID_Param_Init();

      } else {
        KEY1_Status = 0;
        KEY2_Status = 0;
        KEY3_Status = 0;
        KEY4_Status = 0;
        KEY = 0;
      }
    }

  } else {
    // DL_GPIO_setPins(OTHER_PORT, OTHER_TEST_PIN);
    KEY_CNT1 = 0;
  }

  // 按键2
  if (!DL_GPIO_readPins(KEYS_PORT, KEYS_KEY2_PIN)) {
    KEY_CNT2++;
    if (KEY_CNT2 > 3) {
      KEY_CNT2 = 0;
      if (KEY2_Status == 0) {
        KEY2_Status = 1;
        //   Turn_Right_90();
        KEY = 2;

      } else {
        // KEY1_Status = 0;
        // KEY2_Status = 0;
        // KEY3_Status = 0;
        // KEY4_Status = 0;
        KEY = 0;
        //   Turn_Left_90();
      }
    }
  } else {
    KEY_CNT2 = 0;
  }
  // 按键3
  if (!DL_GPIO_readPins(KEYS_PORT, KEYS_KEY3_PIN)) {
    KEY_CNT3++;
    if (KEY_CNT3 > 3) {
      KEY_CNT3 = 0;
      if (KEY3_Status == 0) {
        KEY3_Status = 1;
        KEY = 3;
      } else {
        KEY1_Status = 0;
        KEY2_Status = 0;
        KEY3_Status = 0;
        KEY4_Status = 0;
        KEY = 0;
      }
    }
  } else {
    KEY_CNT3 = 0;
  }

  // 按键4
  if (!DL_GPIO_readPins(KEYS_PORT, KEYS_KEY4_PIN)) {
    KEY_CNT4++;
    if (KEY_CNT4 > 3) {
      KEY_CNT4 = 0;
      if (KEY4_Status == 0) {
        KEY4_Status = 1;
        MPU6050_PID_Param_Init();
        MPU6050_PID.target_val = 180;
        KEY = 4;
      } else {
        KEY1_Status = 0;
        KEY2_Status = 0;
        KEY3_Status = 0;
        KEY4_Status = 0;
        KEY = 0;
      }
    }
  } else {
    KEY_CNT4 = 0;
  }
}

void Sensor_Control() {
  read_sensor_values();

  //   Total_ERROR += error;

  SPWM = PosionPID_realize(&Sensor_PID, error);
  if (SPWM > 30) {
    SPWM = 30;
  } else if (SPWM < -30) {
    SPWM = -30;
  }
}

void MPU6050_Control() {
  atk_ms901m_attitude_data_t attitude_dat;
  // atk_ms901m_gyro_data_t gyro_dat;
  // atk_ms901m_accelerometer_data_t accelerometer_dat;
  // atk_ms901m_magnetometer_data_t magnetometer_dat;
  // atk_ms901m_barometer_data_t barometer_dat;

  atk_ms901m_get_attitude(&attitude_dat, 100);
  int Yaw = Yaw_Handle(attitude_dat.yaw);
  // atk_ms901m_get_gyro_accelerometer(&gyro_dat, &accelerometer_dat,
  //                                   100);
  // atk_ms901m_get_magnetometer(&magnetometer_dat, 100);
  // atk_ms901m_get_barometer(&barometer_dat, 100);

  OLED_ShowSignedNum(4, 1, Yaw, 3);
  OLED_ShowSignedNum(4, 7, MPU_Init_Data, 3);

  MPWM = PosionPID_realize(&MPU6050_PID, Yaw);
  if (MPWM > 30) {
    MPWM = 30;
  } else if (MPWM < -30) {
    MPWM = -30;
  }
}

/**
 * @brief   PID 速度控制
 * @param   None
 * @return  None
 */
void Motor_PID_Control(void) {
  freq = Encoder_Get_Freq();
  gCaptureCnt1 = 0;
  freq2 = Encoder_Get_Freq2();
  gCaptureCnt2 = 0;

  L_Motor_Distance += freq;
  R_Motor_Distance += freq2;
  //   Distance += Get_Distance_Form_Encoder(freq2);
  // speed = Get_Velocity_Form_Encoder(freq);
  // Total_ERROR += freq;

  APWM = addPID_realize(&Motor1_PID, freq);
  BPWM = addPID_realize(&Motor2_PID, freq2);
}
// ---------------------------- 固定函数 -----------------------------------

void Yaw_Init(void) {
  // ------------ 获取陀螺仪的初始方向 -------------
  atk_ms901m_attitude_data_t attitude_dat;
  atk_ms901m_get_attitude(&attitude_dat, 100);
  MPU_Init_Data = Yaw_Handle(attitude_dat.yaw);
  // ------------------- END ---------------------
}

int Yaw_Handle(int yaw) {
  //   int res = yaw - MPU_Init_Data;

  //   if (res > 180) {
  //     res = -res - 180;
  //   } else if (res < -180) {
  //     res = 180 + res;
  //   }

  return (yaw + 179) * 359 / 358;
}

/**
 * @brief  启动
 * @retval : 无
 */
void Start(void) {
  DL_TimerG_startCounter(PWM_12_INST);
  //   DL_TimerG_startCounter(PWM_34_INST);
}

/**
 * @brief  停车
 * @retval : 无
 */
void Stop(void) {
  Motor1_SetSpeed(0);
  Motor2_SetSpeed(0);
}

/**
 * @brief  PID直行
 * @param Speed 直行速度
 * @param PWM 灰度传感器偏差
 * @retval : 无
 */
void PID_Straight(int ASpeed, int BSpeed, int PWM) {
  Motor1_SetSpeed(ASpeed + PWM);
  Motor2_SetSpeed(BSpeed - PWM);
}

/**
 * @brief  直行
 * @param Speed 直行速度
 * @retval : 无
 */
void Straight(int Speed) {
  Motor1_SetSpeed(Speed);
  Motor2_SetSpeed(Speed);
}

/**
 * @brief  左转
 * @param 无
 * @retval : 无
 */
void Turn_Left_90(void) {
  Motor1_SetSpeed(-30);
  Motor2_SetSpeed(30);
}

/**
 * @brief  右转
 * @param 无
 * @retval : 无
 */
void Turn_Right_90(void) {
  Motor1_SetSpeed(30);
  Motor2_SetSpeed(-30);
}

/**
 * @brief  编码器读数转换为轮子速度（mm/s）
 * @param encoder：编码器计数
 * @retval : Velocity 轮子速度
 */
float Get_Velocity_Form_Encoder(int32_t encoder) {
  float Distance, Velocity;
  Distance = 2 * 3.14159 * 32.5 / (2 * 30 * 30); // 单位是mm
  Velocity =
      encoder * Distance / 0.1; // 单位是mm/s。0.1就是编码器计数周期100ms，0.1s

  return Velocity;
}

/**
 * @brief  编码器读数转换为路程
 * @param encoder：编码器总计数
 * @retval : 总距离 mm
 */
float Get_Distance_Form_Encoder(int32_t encoder) {
  float distance, Velocity;
  distance = 2 * 3.14159 * 32.5 / (2 * 364 * 28); // 364
  return encoder * distance / 0.04;
}

void TIMER_0_INST_IRQHandler(void) {
  switch (DL_TimerG_getPendingInterrupt(TIMER_0_INST)) {
  case DL_TIMER_IIDX_ZERO:
    PID_TIMER++;
    // HCSR04_TIMER++;
    LED_TIMER++;
    // MPU6050_TIMER++;

    // ------------- LED ----------
    if (Led_Flag == 1) {
      LED_TIMER++;
    }

    if (LED_TIMER > 20000) {
      LED_TIMER = 0;
      Led_Flag = 0;
    }

    // ------------- END ------------

    if (PID_TIMER >= 1500) {
      PID_TIMER = 0;
      Motor_PID_Control();
    }
    break;
  default:
    break;
  }
}
